import numpy as np
import matplotlib.pyplot as plt
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise

'''
1、输入参数
x : ndarray (dim_x, 1), default = [0,0,0…0] 表示滤波器需要估计的状态向量

F : ndarray (dim_x, dim_x) 表示状态转移方程 x(t)=F*x(t-1)
H : ndarray (dim_z, dim_x) 表示量测方程 z=H*x

P : ndarray (dim_x, dim_x), default eye(dim_x) 表示误差协方差矩阵,x变量之间独立，那么eye

Q : ndarray (dim_x, dim_x), default eye(dim_x) 表示预测噪声（系统噪声）
R : ndarray (dim_z, dim_z), default eye(dim_x) 表示量测噪声


B : ndarray (dim_x, dim_u), default 0 表示控制转移矩阵
　经过上面一步，只有PQRK四个矩阵还未确定了。显然增益矩阵K是不需要初始化的，P是误差矩阵，初始化可以是一个随机的矩阵或者0，只要经过几次的处理基本上就能调整到正常的水平，因此也就只会影响前面几次的滤波结果。

　　Q和R分别是预测和观测状态协方差矩阵，一般可以简单认为系统状态各维之间(即上面的a和b)相互独立，那么Q和R就可以设置为对角阵。而这两个对角线元素的大小将直接影响着滤波结果，若Q的元素远大于R的元素，则预测噪声大，从而更相信观测值，这样可能使得kalman滤波结果与观测值基本一致；反之，则更相信预测，kalman滤波结果会表现得比较规整和平滑；若二者接近，则滤波结果介于前面两者之间，根据实验效果看也缺乏实际使用价值。

　　以上几个矩阵确定后，对于状态x，由于0时刻我们没有任何关于该系统的知识，可以使用0时刻的测量值z0来初始x0，预测从k=1开始；也可以初始化-1时刻的状态，当然这个状态实际是未知的，也就可随机取。2种方式都可以，但使用0时刻测量值来初始化状态，可以使得前面几次预测更准确。
影响滤波结果平滑性的因素是cR/cQ，这个值反映了我们对于预测和观测值的信任程度；其值越大则越相信预测结果，滤波结果平滑性好；反之则越相信观测结果，滤波结果越偏向于观测值

2、可选参数
alpha : float
Assign a value > 1.0 to turn this into a fading memory filter.

3.可参看参数
K : ndarray 卡尔曼增益
y : ndarray  残差，实际量测和当前估计状态映射到量测空间的量测之间的差异 =(z - Hx)
S : ndarray 系统不确定性映射到测量空间 =HPH’ + R 。
likelihood : float 上次测量更新的似然。
log_likelihood : float 上次测量更新的对数似然。




'''
np.random.seed(2)


def demo():
    dim_x=2
    dim_z=1
    dt = 0.1
    F = np.array([[1, dt], [0, 1]])
    # Q = 1e-2 * np.array([[1 / 4 * dt ** 4, 1 / 2 * dt ** 3], [1 / 2 * dt ** 3, dt ** 2]])
    Q =  np.array([[1 / 4 * dt ** 4, 1 / 2 * dt ** 3], [1 / 2 * dt ** 3, dt ** 2]])/10  #状态也就是实际值的误差
    R = 9 #测量值的误差
    itr = 100
    
    # 造数据
    real_state = []
    x = np.array([10, 5]).reshape(2, 1)
    
    for i in range(itr):
        real_state.append(x[0, 0])
        # 考虑状态误差
        x = np.dot(F, x) + np.random.multivariate_normal(mean=(0, 0), cov=Q).reshape(2, 1)
    # 考虑测量误差
    measurements = [x + np.random.normal(0, R) for x in real_state]
    # 造数据结束

    # initialization
    P = np.eye(dim_x)
    x = np.random.multivariate_normal(mean=(10, 5), cov=P).reshape(2, 1)

    # filter
    kf = KalmanFilter(dim_x=2, dim_z=1)  # dim_x:隐状态大小，dim_z:量测大小
    # 定义参数
    kf.x = x  # 初始状态[位置,速度]
    kf.F = F  # 状态转移矩阵
    kf.H = np.array([[1., 0.]])  # 量测矩阵 z=H*x
    # kf.P = P  # 初始状态协方差
    kf.R = R  # 量测噪声
    kf.Q = Q_discrete_white_noise(dim=2, dt=dt, var=1e-2)  # 过程（系统）噪声
    
    filter_result = list()
    filter_result.append(x)
    for i in range(1, itr):
        z = measurements[i]
        kf.predict()
        kf.update(z)
        filter_result.append(kf.x)
    filter_result = np.squeeze(np.array(filter_result))
    
    return measurements, real_state, filter_result


def plot_result(measurements, real_state, filter_result):
    plt.figure(figsize=(8, 4))
    plt.plot(range(1, len(measurements)), measurements[1:], label='Measurements')
    plt.plot(range(1, len(real_state)), real_state[1:], label='Real statement')
    plt.plot(range(1, len(filter_result)), np.array(filter_result)[1:, 0], label='Kalman Filter')
    plt.legend()
    plt.xlabel('Time', fontsize=14)
    plt.ylabel('velocity [m]', fontsize=14)
    plt.show()
    
    plt.figure(figsize=(8, 4))
    plt.axhline(5, label='Real statement')  # , label='$GT_x(real)$'
    plt.plot(range(1, len(filter_result)), np.array(filter_result)[1:, 1], label='Kalman Filter')
    plt.legend()
    plt.xlabel('Time', fontsize=14)
    plt.ylabel('velocity [m]', fontsize=14)
    plt.show()


if __name__ == '__main__':
    measurements, real_state, filter_result = demo()
    plot_result(measurements, real_state, filter_result)

